//
// Created by cch on 2022/5/1.
//

#include "gps_processer.h"

#include <utility>

IMUGPS::GpsProcesser::GpsProcesser(const Eigen::Vector3d &I_P_GPS)
    : I_P_GPS_(std::move(I_P_GPS)) {}

void IMUGPS::GpsProcesser::Update(const Eigen::Vector3d &init_lla,
                                  const GpsDataPtr &gpsDataPtr, State *state) {

  // 观测矩阵
  Eigen::Matrix<double, 3, 15> H;
  // 残差
  Eigen::Vector3d residual;
  // 计算雅克比矩阵
  computeResidualAndJacobian(init_lla, H, residual, state, gpsDataPtr);

  // 取出gps的协方差
  const Eigen::MatrixXd V = gpsDataPtr->cov;
  // 取出IMU的协方差
  const Eigen::MatrixXd P = state->cov;
  // 计算卡尔曼增益
  const Eigen::MatrixXd K =
      P * H.transpose() * (H * P * H.transpose() + V).inverse();
  // 更新状态
  const Eigen::VectorXd delta_x = K * residual;
  const Eigen::MatrixXd I_KH =
      Eigen::Matrix<double, 15, 15>::Identity() - K * H;
  // 更新协方差
  state->cov = I_KH * P * I_KH.transpose() + K * V * K.transpose();
  // 更新状态
  AddDeltaToState(delta_x, state);
}

void IMUGPS::GpsProcesser::computeResidualAndJacobian(
    const Eigen::Vector3d &init_lla, Eigen::Matrix<double, 3, 15> &jacobian,
    Eigen::Vector3d &residual, const State *state,
    const GpsDataPtr &gpsDataPtr) {

  const Eigen::Vector3d G_P_I = state->G_P_I;
  const Eigen::Matrix3d G_R_I = state->G_R_I;

  Eigen::Vector3d G_P_GPS;
  // 转到IMU坐标系下
  IMUGPS::ConvertLLAtoENU(init_lla, gpsDataPtr->lla, G_P_GPS);

  // 残差 = 观测-预测
  residual = G_P_GPS - (G_P_I + G_R_I * I_P_GPS_);

  jacobian.setZero();
  jacobian.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
  jacobian.block<3, 3>(0, 6) = -G_R_I * IMUGPS::GetSkewMatrix(I_P_GPS_);
}

void IMUGPS::GpsProcesser::AddDeltaToState(
    const Eigen::Matrix<double, 15, 1> &delta_x, State *state) {
  state->G_P_I += delta_x.block<3, 1>(0, 0);
  state->G_V_I += delta_x.block<3, 1>(3, 0);
  state->acc_bias += delta_x.block<3, 1>(9, 0);
  state->gyro_bias += delta_x.block<3, 1>(12, 0);
  state->G_R_I *= Eigen::AngleAxisd(delta_x.block<3, 1>(6, 0).norm(),
                                    delta_x.block<3, 1>(6, 0).normalized())
                      .toRotationMatrix();
}